
#include "clib_coded_base32.h"

#define CLIB_CODED_BASE32_CHAR_TO_UPPER(x)               (((x) > 0x60 && (x) < 0x7b)? (x) - 0x20 : (x))

size_t clib_coded_base32_encode(clib_uint8_t const *in_data, size_t in_size, char *out_data, size_t out_size) {
    clib_check_if_true_return_value(in_data == NULL, 0);
    clib_check_if_true_return_value(out_data == NULL, 0);
    clib_check_if_true_return_value(in_size >= UINT32_MAX / 4, 0);
    clib_check_if_true_return_value(out_size < CLIB_CODED_BASE32_OUTPUT_MIN(in_size), 0);
    static char const table[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZ234567";
    size_t i = 0;
    clib_uint8_t w = 0;
    size_t idx = 0;
    char *pb = out_data;
    for (; i < in_size;) {
        if (idx > 3) {
            w = (in_data[i] & (0xFF >> idx));
            idx = (idx + 5) & 0x07;
            w <<= idx;
            if (i < in_size - 1)
                w |= in_data[i + 1] >> (8 - idx);
            i++;
        } else {
            w = (in_data[i] >> (8 - (idx + 5))) & 0x1F;
            idx = (idx + 5) & 0x07;
            if (idx == 0) i++;
        }
        *pb++ = table[w];
    }
    size_t result_length = (pb - out_data);
    size_t real_length = clib_align8(result_length);
    for (int j = 0; j < real_length - result_length; ++j) {
        *pb++ = '=';
    }
    *pb = '\0';
    return real_length;
}


size_t clib_coded_base32_decode(char const *in_data, size_t in_size, clib_uint8_t *out_data, size_t out_size) {
    clib_check_if_true_return_value(in_data == NULL, 0);
    clib_check_if_true_return_value(out_data == NULL, 0);
    static clib_uint8_t const table[43][2] =
            {
                    {'0', 0xFF},
                    {'1', 0xFF},
                    {'2', 0x1A},
                    {'3', 0x1B},
                    {'4', 0x1C},
                    {'5', 0x1D},
                    {'6', 0x1E},
                    {'7', 0x1F},
                    {'8', 0xFF},
                    {'9', 0xFF},
                    {':', 0xFF},
                    {';', 0xFF},
                    {'<', 0xFF},
                    {'=', 0xFF},
                    {'>', 0xFF},
                    {'?', 0xFF},
                    {'@', 0xFF},
                    {'A', 0x00},
                    {'B', 0x01},
                    {'C', 0x02},
                    {'D', 0x03},
                    {'E', 0x04},
                    {'F', 0x05},
                    {'G', 0x06},
                    {'H', 0x07},
                    {'I', 0x08},
                    {'J', 0x09},
                    {'K', 0x0A},
                    {'L', 0x0B},
                    {'M', 0x0C},
                    {'N', 0x0D},
                    {'O', 0x0E},
                    {'P', 0x0F},
                    {'Q', 0x10},
                    {'R', 0x11},
                    {'S', 0x12},
                    {'T', 0x13},
                    {'U', 0x14},
                    {'V', 0x15},
                    {'W', 0x16},
                    {'X', 0x17},
                    {'Y', 0x18},
                    {'Z', 0x19}
            };
    memset(out_data, 0, out_size);
    size_t i = 0;
    clib_uint8_t w = 0;
    size_t idx = 0;
    clib_uint8_t *op = out_data;
    for (; i < in_size; ++i) {
        clib_int32_t lookup = CLIB_CODED_BASE32_CHAR_TO_UPPER(in_data[i]) - '0';
        if (lookup < 0 || lookup >= 43) w = 0xff;
        else w = table[lookup][1];
        if (w == 0xff) continue;

        if (idx <= 3) {
            idx = (idx + 5) & 0x07;
            if (idx == 0) *op++ |= w;
            else *op |= w << (8 - idx);
        } else {
            idx = (idx + 5) & 0x07;
            *op++ |= (w >> idx);
            *op |= w << (8 - idx);
        }
    }
    return (op - out_data);
}
